// #include "Control.hpp"

// #define MAIN 0
// #define IMG 1
// #define PID 2
// #define MOVE 3

// int currentState = MAIN;
// int currentMenu = 0;
// int currentIMG = 0;
// int currentPID = 0;

// char str1[32], str2[32], str3[32], str4[32];
// char str5[32], str6[32], str7[32], str8[32];
// char str9[32], str10[32], str11[32], str12[32];


// // IMG
// unsigned short i = 130;
// unsigned short motor_goal_666 = 500;
// unsigned short qianzhan_dx = 1;
// unsigned short servowyz ;
// unsigned short L, R;

// // PID
// unsigned short kp_servo_f = 12;
// unsigned short kpp = 18;
// unsigned short kd_servo_f = 6;
// unsigned short kp_servo_l = 6;
// unsigned short kd_servo_l = 12;

// void handleMAIN();
// void handleIMG();
// void handlePID();
// void handleMOVE();

// void displayMAIN();
// void displayIMG();
// void displayPID();
// void displayMOVE();

// float qianzhan_dy = qianzhan_dx / 100;
// int prev_button1 = 0;
// int prev_button2 = 0;
// int prev_button3 = 0;
// int prev_button4 = 0;


// bool executeBlock = false;

// void Control()
// {
//     TFTSPI_dri_init(1);
//     /**********************************  camera   *******************************/
//     VideoCapture cap(0);
//     if (!cap.isOpened())
//     {
//         cerr << "Failed to open camera!" << endl;
//     }

//     cap.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));
//     cap.set(CAP_PROP_FRAME_WIDTH, 160);
//     cap.set(CAP_PROP_FRAME_HEIGHT, 120);
//     cap.set(CAP_PROP_FPS, 120);

//     // 获取摄像头图像宽高和帧率
//     int frame_width = cap.get(cv::CAP_PROP_FRAME_WIDTH);
//     int frame_height = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
//     double frame_fps = cap.get(cv::CAP_PROP_FPS);

//     int sockfd = socket(AF_INET, SOCK_DGRAM, 0);

//     if (sockfd < 0)
//     {
//         cerr << "Failed to create socket!" << endl;
//     }

//     // 配置目标地址
//     struct sockaddr_in dest_addr;
//     memset(&dest_addr, 0, sizeof(dest_addr));
//     dest_addr.sin_family = AF_INET;
//     dest_addr.sin_port = htons(UDP_PORT);
//     uint8_t data[120][160];

//     if (inet_pton(AF_INET, DST_IP, &dest_addr.sin_addr) <= 0)
//     {
//         cerr << "Invalid IP address!" << endl;
//         close(sockfd);
//     }
//     vector<int> encode_params = {IMWRITE_JPEG_QUALITY, 100};

//     Mat frame;
//     auto start = chrono::high_resolution_clock::now();
//     int frameCount = 0;

//     // bool executeBlock = false;

//     while (1)
//     {
//         // 捕获帧
//     //     if (!cap.read(frame) || frame.empty())
//     //     {
//     //         cerr << "Failed to capture frame!" << endl;
//     //         continue;
//     //     }

//     //     frameCount++;
//     //     auto end = chrono::high_resolution_clock::now();
//     //     chrono::duration<double> elapsed = end - start;
//     //     double fps = frameCount / elapsed.count();

//     //     cout << "FPS: " << fps << endl;

//     //     /*******************************  processImage  ***************************/

//     //     float angle, WC;
//     //     int chujie, zerbra, flag_left, flag_right;
//     //     int button44 = key44();
//     //     int button1 = key1();
//     //     int button2 = key2();
//     //     int button3 = key3();
//     //     int button4 = key4();

//     //     Mat view = applyPerspectiveTransform(frame);

//     //     memset(str1, 0, sizeof(str1));
//     //     memset(str5, 0, sizeof(str5));
//     //     memset(str2, 0, sizeof(str2));
//     //     memset(str3, 0, sizeof(str3));
//     //     memset(str4, 0, sizeof(str4));

//     //     // 图像预处理
//     //     Mat gray, binary;
//     //     cvtColor(view, gray, COLOR_BGR2GRAY);
//     //     GaussianBlur(gray, gray, Size(5, 5), 3, 3);
//     //     threshold(gray, binary, i, 255, THRESH_BINARY);
//     //     // uint8_t threshold_ = otsuThreshold(gray);
//     //     // threshold(gray, binary, threshold_, 255, THRESH_BINARY);

//     //     zerbra = zebra(binary);

//     //     float lastwc = 0.0;
//     //     WC = findWhiteColumn(binary); // 长白列

//     //     // int WC_diff = iswandao(WC, lastwc);
//     //     lastwc = WC;
//     //     chujie = isParking(binary);

//     //     // 创建标记图像
//     //     Mat colorResult;
//     //     cvtColor(binary, colorResult, COLOR_GRAY2BGR);

//     //     // 输出最长白列的长度和前N列的平均长度
//     //     putText(colorResult, "WC: " + to_string(int(WC)),
//     //             Point(10, 60), FONT_HERSHEY_SIMPLEX, 0.3, PURPLE, 0.5);

//     //     putText(colorResult, "z: " + to_string(int(zerbra)),
//     //             Point(10, 80), FONT_HERSHEY_SIMPLEX, 0.3, PURPLE, 0.5);

//     //     // 十字识别
//     //     int rowsWithWhiteOver = 0;

//     //     // 第一次遍历：统计满足条件的行数
//     //     for (int y = 19; y <= 59; ++y)
//     //     {
//     //         int whiteCount = 0;
//     //         for (int x = 0; x < binary.cols; ++x)
//     //         {
//     //             if (binary.at<uchar>(y, x) == 255)
//     //             {
//     //                 whiteCount++;
//     //             }
//     //         }
//     //         if (whiteCount > 110)
//     //         {
//     //             rowsWithWhiteOver++;
//     //         }
//     //     }
//     //     bool shouldMark = (rowsWithWhiteOver >= 20);

//     //     // 处理每一行，寻找左右边缘并追踪
//     //     EdgePointsMap edgePoints; // 存储每行的边缘点
//     //     vector<Point> centerPoints;

//     //     // 用于存储历史中心点的滑动窗口
//     //     deque<int> centerXHistory;
//     //     // 滑动窗口大小
//     //     const int WINDOW_SIZE = 5;
//     //     // 中点跳变阈值
//     //     const int JUMP_THRESHOLD = 20;

//     //     // 上一行的平滑后中心点
//     //     int smoothedPrevCenterX = -1;
//     //     // 上一行的原始中心点
//     //     int rawPrevCenterX = -1;

//     //     // 从图像底部往上处理每一行
//     //     for (int y = binary.rows - 1; y >= 0; --y)
//     //     {
//     //         int middleCol = binary.cols / 2;
//     //         // 使用平滑后的中心点作为搜索起点
//     //         int startX = (smoothedPrevCenterX != -1) ? smoothedPrevCenterX : middleCol;

//     //         // 查找左侧边缘点（从startX往左）
//     //         int leftX = -1;
//     //         for (int x = startX; x > 0; --x)
//     //         {
//     //             if (binary.at<uchar>(y, x) == 255 && binary.at<uchar>(y, x - 1) == 0)
//     //             {
//     //                 leftX = x;
//     //                 break;
//     //             }
//     //         }

//     //         // 查找右侧边缘点（从startX往右）
//     //         int rightX = -1;
//     //         for (int x = startX; x < binary.cols - 1; ++x)
//     //         {
//     //             if (binary.at<uchar>(y, x) == 255 && binary.at<uchar>(y, x + 1) == 0)
//     //             {
//     //                 rightX = x;
//     //                 break;
//     //             }
//     //         }

//     //         // 标记边缘点
//     //         Point leftPt(leftX, y);
//     //         Point rightPt(rightX, y);

//     //         if (leftPt.x != -1)
//     //         {
//     //             circle(colorResult, leftPt, 1, RED, -1); // 红色标注左边沿
//     //         }

//     //         if (rightPt.x != -1)
//     //         {
//     //             circle(colorResult, rightPt, 1, BLUE, -1); // 蓝色标注右边沿
//     //         }

//     //         // 如果左右点都存在，则计算中点
//     //         if (leftPt.x != -1 && rightPt.x != -1)
//     //         {
//     //             int rawCurrentCenterX = (leftPt.x + rightPt.x) / 2;
//     //             Point rawCenter(rawCurrentCenterX, y);

//     //             // 应用跳变检测
//     //             bool isJump = false;
//     //             if (rawPrevCenterX != -1 && abs(rawCurrentCenterX - rawPrevCenterX) > JUMP_THRESHOLD)
//     //             {
//     //                 isJump = true;
//     //                 // 当检测到跳变时，使用历史平均值作为当前值
//     //                 if (!centerXHistory.empty())
//     //                 {
//     //                     int sum = 0;
//     //                     for (int val : centerXHistory)
//     //                     {
//     //                         sum += val;
//     //                     }
//     //                     rawCurrentCenterX = sum / centerXHistory.size();
//     //                     rawCenter.x = rawCurrentCenterX;
//     //                 }
//     //             }

//     //             // 添加到历史记录
//     //             centerXHistory.push_back(rawCurrentCenterX);
//     //             // 保持窗口大小固定
//     //             if (centerXHistory.size() > WINDOW_SIZE)
//     //             {
//     //                 centerXHistory.pop_front();
//     //             }

//     //             // 计算滑动平均
//     //             int sum = 0;
//     //             for (int val : centerXHistory)
//     //             {
//     //                 sum += val;
//     //             }
//     //             int smoothedCurrentCenterX = sum / centerXHistory.size();
//     //             Point smoothedCenter(smoothedCurrentCenterX, y);

//     //             // 标记平滑后的中点
//     //             centerPoints.push_back(smoothedCenter);
//     //             circle(colorResult, smoothedCenter, 0.6, GREEN, -1);

//     //             // 更新中心点
//     //             if (!isJump)
//     //             {
//     //                 rawPrevCenterX = rawCurrentCenterX;
//     //             }
//     //             smoothedPrevCenterX = smoothedCurrentCenterX;
//     //         }
//     //     }

//     //     static float prevAngle = 0.0; // 用于存储前一次计算的angle值
//     //     const float maxDelta = 18.0;  // 设置一个突变点阈值

//     //     if (!centerPoints.empty())
//     //     {
//     //         const int imageMidX = colorResult.cols / 2;

//     //         int imageMidY;
//     //         int imageMidY_;

//     //         if (WC < 105)
//     //         {
//     //             imageMidY_ = colorResult.rows * 0.21 + qianzhan_dx; // 图像的中间Y位置
//     //         }
//     //         else if (WC < 110)
//     //         {
//     //             imageMidY_ = colorResult.rows * 0.23 + qianzhan_dx; // 图像的中间Y位置
//     //         }
//     //         else
//     //         {
//     //             imageMidY_ = colorResult.rows * 0.25 + qianzhan_dx; // 图像的中间Y位置
//     //         }
//     //         circle(colorResult, Point(imageMidX, imageMidY_), 2, RED, -1); // 红色标注图像的中间位置

//     //         // 动态调整的参数
//     //         float decayRateBelow = 0.1; // 权重衰减率（每像素衰减2%），下方
//     //         float decayRateAbove = 0.08;
//     //         const float baseWeight = 1.0; // 基础权重系数

//     //         imageMidY = imageMidY_ + 0.03;

//     //         // 进十字修改权重
//     //         if (shouldMark)
//     //         {
//     //             imageMidY = imageMidY_ + 50;
//     //             decayRateBelow = 0.02;
//     //             decayRateAbove = 1;
//     //         }

//     //         float weightedSum = 0.0;
//     //         float totalWeight = 0.0;

//     //         // 绘制图像中线
//     //         line(colorResult, Point(imageMidX, 0), Point(imageMidX, colorResult.rows),
//     //              Scalar(0, 255, 255), 1); // 黄色中线

//     //         for (const auto &pt : centerPoints)
//     //         {
//     //             int diff = pt.x - imageMidX;

//     //             float weight;
//     //             if (pt.y > imageMidY)
//     //             {
//     //                 // 下方
//     //                 weight = baseWeight * exp(-decayRateBelow * (pt.y - imageMidY));
//     //             }
//     //             else
//     //             {
//     //                 // 上方
//     //                 weight = baseWeight * exp(-decayRateAbove * (imageMidY - pt.y));
//     //             }

//     //             weightedSum += diff * weight;
//     //             totalWeight += weight;

//     //             // 用圆点表示权重大小，weight越大圆点半径越大
//     //             float radius = weight * 5.0f;                                             // 可根据效果调整放大系数
//     //             circle(colorResult, pt, static_cast<int>(radius), Scalar(255, 0, 0), -1); // 蓝色实心圆
//     //         }

//     //         if (totalWeight > 0)
//     //         {
//     //             int avgDiff = (weightedSum / totalWeight);

//     //             // 判断是否为突变点
//     //             if (abs(avgDiff - prevAngle) > maxDelta)
//     //             {
//     //                 angle = (prevAngle + avgDiff) * 0.5;
//     //             }
//     //             else
//     //             {
//     //                 // 否则，更新angle值
//     //                 angle = avgDiff;
//     //             }
//     //             prevAngle = angle;
//     //             putText(colorResult, "angle: " + to_string(int(angle)),
//     //                     Point(10, 40), FONT_HERSHEY_SIMPLEX, 0.3, PURPLE, 2);
//     //             circle(colorResult, Point(80, 105), 2, RED, -1);
//     //         }
//     //     }
//     //     else
//     //     {
//     //         // 如果centerPoints为空 保持前一次的angle值
//     //         angle = prevAngle;
//     //     }
//     //     if (colorResult.empty())
//     //     {
//     //         continue;
//     //     }
//     //     // 编码器读取速度
//     //     float LSpeed = -R_encoder_value();
//     //     float RSpeed = -L_encoder_value();

//     //     /******************************** UDP发送图像 *******************************/

//     //     // if (!sendProcessedImageViaUDP(processedFrame, sockfd, dest_addr, encode_params))
//     //     // {
//     //     //     cerr << "Failed to send processed image via UDP." << endl;
//     //     // }

//     //     /********************************    drive    ****************************/

//     //     // 参数初始化
//     //     int servoOutput = 0;
//     //     int servoDutyCycle = servo_angle_mid;
//     //     int motorL = 0;
//     //     int motorR = 0;

//     //     // output = kp,kp_fang,kd,kd,ki;
//     //     erciPD pdf(kp_servo_f / 10 + 0.8, kpp / 1000, kd_servo_f / 10 + 0.5, 0, 0); //  弯道
//     //     erciPD pdl(kp_servo_f / 10 + 1, 0, kd_servo_l / 10, 0, 0);                  //  直道

//     //     // 设置触发按键（例如：当button1按下时切换执行状态）
//     //     if (button1 == 0 && button2 == 1 && button3 == 1 && button4 == 1)
//     //     { // 检测上升沿
//     //         TFTSPI_dir_flush();
//     //         TFTSPI_dir_cls(u16BLACK);
//     //         executeBlock = !executeBlock; // 切换执行状态
//     //         Delay_Ms(20);                 // 消抖
//     //     }

//     //     // 当executeBlock为true时，执行目标代码块
//     //     if (executeBlock)
//     //     {
//     //         /**************** 目标代码块（原数据传输部分） ****************/
//     //         // if(button1 == 1&& button2 == 0 && button3 == 1 && button4 == 1){
//     //         for (int i = 0; i < 120; i++)
//     //         {
//     //             for (int j = 0; j < 160; j++)
//     //             {
//     //                 data[i][j] = binary.at<unsigned char>(i, j);
//     //             }
//     //         }
//     //         // }

//     //         TFTSPI_dir_binRoad(0, 0, 120, 160, (uint8_t *)data);

//     //         // 阈值调整逻辑
//     //         if (button3 == 0 && button1 == 1 && button2 == 1 && button4 == 1)
//     //         {
//     //             i = i + 5;
//     //         }
//     //         else if (button4 == 0 && button1 == 1 && button2 == 1 && button3 == 1)
//     //         {
//     //             i = i - 5;
//     //         }
//     //         sprintf(str7, "Threshold : %05d", i);
//     //         TFTSPI_dir_P6X8Str(2, 16, str7, u16WHITE, u16BLACK);
//     //         // PID参数调整逻辑
//     //         if (button1 == 0 && button3 == 0)
//     //         {
//     //             kp_servo_f += 1;
//     //         }
//     //         else if (button1 == 0 && button4 == 0)
//     //         {
//     //             kp_servo_f -= 1;
//     //         }
//     //         else if (button2 == 0 && button3 == 0)
//     //         {
//     //             kd_servo_f += 1;
//     //         }
//     //         else if (button2 == 0 && button4 == 0)
//     //         {
//     //             kd_servo_f -= 1;
//     //         }

//     //         sprintf(str2, "kp_servo: %05d", kp_servo_f);
//     //         TFTSPI_dir_P6X8Str(2, 17, str2, u16RED, u16BLUE);
//     //         sprintf(str3, "kd_servo: %05d", kd_servo_f);
//     //         TFTSPI_dir_P6X8Str(2, 18, str3, u16RED, u16BLUE);

//     //         sprintf(str4, "motor: %05d", motor_goal_666);
//     //         TFTSPI_dir_P8X8Str(2, 19, str4, u16RED, u16BLUE);
//     //         if (button1 == 0 && button2 == 1 && button3 == 1 && button4 == 1)
//     //         {
//     //             sleep(1);
//     //             motor_goal_666 += 10;
//     //         }
//     //         else if (button1 == 1 && button2 == 0 && button3 == 1 && button4 == 1)
//     //         {
//     //             motor_goal_666 -= 10;
//     //         }
//     //         /**************** 目标代码块结束 ****************/
//     //     }

//     //     incrPIDL piL(50, 2, 1);
//     //     incrPIDR piR(50, 2, 1);

//     //     int motor_goal = motor_goal_666;

//     //     // cout << "\n Encoder_L:  " << LSpeed << "   Encoder_R: " << RSpeed << endl;

//     //     if (chujie == 1)
//     //     {
//     //         Servo_value(150000);
//     //         LMotor_value(0);
//     //         RMotor_value(0);
//     //     }
//     //     else
//     //     {
//     //         int motorf = motor_goal * 0.8;
//     //         int motorl = motor_goal * 0.7;
//     //         int motor = motor_goal;

//     //         if (zerbra > 110)
//     //         {
//     //             Servo_value(150000);
//     //             LMotor_value(30000);
//     //             RMotor_value(30000);
//     //         }
//     //         else
//     //         {
//     //             servoOutput = pdf.update(angle_goal, angle, 0);
//     //             if (WC < 108)
//     //             {
//     //                 servoOutput = pdf.update(angle_goal, angle, 0);
//     //                 if (WC < 80)
//     //                 {
//     //                     if (angle < -20)
//     //                     {
//     //                         motorL = piL.update(motorl * 0.86, LSpeed);
//     //                         motorR = piR.update(motorf * 1.6, RSpeed);
//     //                     }
//     //                     else if (angle > 20)
//     //                     {
//     //                         motorL = piL.update(motorf * 1.7, LSpeed);
//     //                         motorR = piR.update(motorl * 0.86, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorl, LSpeed);
//     //                         motorR = piR.update(motorl, RSpeed);
//     //                     }
//     //                 }
//     //                 else if (WC < 90)
//     //                 {
//     //                     if (angle < -20)
//     //                     {
//     //                         motorL = piL.update(motorl * 0.9, LSpeed);
//     //                         motorR = piR.update(motorf * 1.5, RSpeed);
//     //                     }
//     //                     else if (angle > 20)
//     //                     {
//     //                         motorL = piL.update(motorf * 1.5, LSpeed);
//     //                         motorR = piR.update(motorl * 0.9, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorl, LSpeed);
//     //                         motorR = piR.update(motorl, RSpeed);
//     //                     }
//     //                 }
//     //                 else if (WC < 100)
//     //                 {
//     //                     if (angle < -20)
//     //                     {
//     //                         motorL = piL.update(motorl * 0.95, LSpeed);
//     //                         motorR = piR.update(motorf * 1.35, RSpeed);
//     //                     }
//     //                     else if (angle > 20)
//     //                     {
//     //                         motorL = piL.update(motorf * 1.35, LSpeed);
//     //                         motorR = piR.update(motorl * 0.95, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorl, LSpeed);
//     //                         motorR = piR.update(motorl, RSpeed);
//     //                     }
//     //                 }
//     //                 else
//     //                 {
//     //                     if (angle < -10)
//     //                     {
//     //                         motorL = piL.update(motorl, LSpeed);
//     //                         motorR = piR.update(motorf * 1.2, RSpeed);
//     //                     }
//     //                     else if (angle > 10)
//     //                     {
//     //                         motorL = piL.update(motorf * 1.2, LSpeed);
//     //                         motorR = piR.update(motorl, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                 }
//     //             }
//     //             else if (WC < 116)
//     //             {
//     //                 servoOutput = pdl.update(angle_goal, angle, 0);
//     //                 if (WC < 110)
//     //                 {
//     //                     if (angle < -13)
//     //                     {
//     //                         motorL = piL.update(motorf * 0.9, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                     else if (angle > 13)
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf * 0.9, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                 }
//     //                 else if (WC < 112)
//     //                 {
//     //                     if (angle < -10)
//     //                     {
//     //                         motorL = piL.update(motorf * 0.96, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                     else if (angle > 10)
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf * 0.96, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                 }
//     //                 else if (WC < 114)
//     //                 {
//     //                     if (angle < -8)
//     //                     {
//     //                         motorL = piL.update(motorf * 0.98, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                     else if (angle > 8)
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf * 0.98, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                 }
//     //                 else
//     //                 {
//     //                     if (angle < -10)
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motor, RSpeed);
//     //                     }
//     //                     else if (angle > 10)
//     //                     {
//     //                         motorL = piL.update(motor, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                     else
//     //                     {
//     //                         motorL = piL.update(motorf, LSpeed);
//     //                         motorR = piR.update(motorf, RSpeed);
//     //                     }
//     //                 }
//     //             }
//     //             else
//     //             {
//     //                 servoOutput = pdl.update(angle_goal, angle, 0);
//     //                 motorL = piL.update(motor, LSpeed);
//     //                 motorR = piR.update(motor, RSpeed);
//     //             }
//     //         }
//     //     }
//     //     servoDutyCycle = 150000 + servoOutput * 100;

//     //     servowyz = servoDutyCycle;
//     //     L = motorL;
//     //     R = motorR;

//     //     // 舵机电机输出
//     //     Servo_value(servoDutyCycle);
//     //     LMotor_value(motorL);
//     //     RMotor_value(motorR);

//     //     // cout << "motorDutyCycle_L: " << motorL << "   motorDutyCycle_R: " << motorR << endl;
//     //     // cout << " WC" << WC << endl;

//     //     // 电机方向（不动）
//     //     LMotor_dir();
//     //     RMotor_dir();
//     //     prev_button1 = button1;
//     //     prev_button2 = button2;
//     //     prev_button3 = button3;
//     //     prev_button4 = button4;
//     // }
//     cap.release();
// }

// void loop()
// {
//     switch (currentState)
//     {
//     case 0:
//         handleMAIN();
//         break;
//     case 1:
//         handleIMG();
//         break;
//     case 2:
//         handlePID();
//         break;
//     case 3:
//         handleMOVE();
//         break;
//     }
//     Delay_Ms(200); // 防抖延时
// }

// void handleMAIN()
// {
//     int button1 = key1();
//     int button2 = key2();
//     int button3 = key3();

//     if (button2 == 0 && button1 == 1 && button3 == 1)
//     { // 下移
//         currentMenu = (currentMenu + 1) % 3;
//         displayMAIN();
//     }

//     if (button3 == 0 && button1 == 1 && button1 == 1)
//     { // 上移
//         currentMenu = (currentMenu + 2) % 3;
//         displayMAIN();
//     }

//     if (button1 == 0 && button2 == 1 && button3 == 1)
//     { // 确认
//         currentState = currentMenu + 1;
//         switch (currentState)
//         {
//         case 1:
//             displayIMG();
//             break;
//         case 2:
//             displayPID();
//             break;
//         case 3:
//             displayMOVE();
//             break;
//         }
//     }
// }

// void handleIMG()
// {

//     int button1 = key1();
//     int button2 = key2();
//     int button3 = key3();
//     int button4 = key4();

//     static bool editingItem = false;

//     if (!editingItem)
//     {
//         if (button2 == 0)
//         {
//             currentPID = (currentPID + 2) % 3;
//             displayIMG;
//         }

//         if (button3 == 0)
//         {
//             currentPID = (currentPID + 1) % 3;
//             displayIMG();
//         }

//         // 进入编辑模式
//         if (button1 == 0)
//         {
//             editingItem = true;
//         }

//         // 返回主菜单
//         if (button4 == 0)
//         {
//             currentState = 0;
//             displayMAIN();
//         }
//     }
//     else
//     {
//         switch (currentIMG)
//         {
//         case 0:
//             if (button2 == 0)
//             {
//                 i = i + 5;
//             }
//             else if (button3 == 0)
//             {
//                 i = i - 5;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str7, "Threshold : %05d", i);
//             TFTSPI_dir_P6X8Str(2, 16, str7, u16WHITE, u16BLACK);
//             break;
//         case 1:
//             if (button2 == 0)
//             {
//                 motor_goal_666 += 10;
//             }
//             else if (button3 == 0)
//             {
//                 motor_goal_666 -= 10;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str4, "motor: %05d", motor_goal_666);
//             TFTSPI_dir_P8X8Str(2, 19, str4, u16RED, u16BLUE);
//             break;
//         case 2:
//             if (button2 == 0)
//             {
//                 qianzhan_dx += 1;
//             }
//             else if (button3 == 0)
//             {
//                 qianzhan_dx -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str5, "qianzhan_dx: %05d", qianzhan_dx);
//             TFTSPI_dir_P8X8Str(2, 20, str5, u16RED, u16BLUE);
//             break;
//         }
//     }
// }

// void handlePID()
// {
//     int button1 = key1();
//     int button2 = key2();
//     int button3 = key3();
//     int button4 = key4();

//     static bool editingItem = false;

//     if (!editingItem)
//     {
//         if (button2 == 0)
//         {
//             currentPID = (currentPID - 1 + 5) % 5;
//             displayPID();
//         }

//         if (button3 == 0)
//         {
//             currentPID = (currentPID + 1) % 5;
//             displayPID();
//         }

//         // 进入编辑模式
//         if (button1 == 0)
//         {
//             editingItem = true;
//         }

//         // 返回主菜单
//         if (button4 == 0)
//         {
//             currentState = 0;
//             displayMAIN();
//         }
//     }
//     else
//     {
//         // 编辑当前项
//         switch (currentPID)
//         {
//         case 0: // kp_servo
//             if (button2 == 0)
//             {
//                 kp_servo_f += 1;
//             }
//             else if (button3 == 0)
//             {
//                 kp_servo_f -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str2, "kp_servo: %05d", kp_servo_f);
//             TFTSPI_dir_P6X8Str(2, 17, str2, u16RED, u16BLUE);
//             break;

//         case 1: // kpp
//             if (button2 == 0)
//             {
//                 kpp += 1;
//             }
//             else if (button3 == 0)
//             {
//                 kpp -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str3, "kd_servo: %05d", kpp);
//             TFTSPI_dir_P6X8Str(2, 18, str3, u16RED, u16BLUE);
//             break;

//         case 2: // kd_servo_f
//             if (button2 == 0)
//             {
//                 kd_servo_f += 1;
//             }
//             else if (button3 == 0)
//             {
//                 kd_servo_f -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str4, "motor: %05d", kd_servo_l);
//             TFTSPI_dir_P8X8Str(2, 19, str4, u16RED, u16BLUE);
//             break;
//         case 3: // kp_servo_l
//             if (button2 == 0)
//             {
//                 kp_servo_l += 1;
//             }
//             else if (button3 == 0)
//             {
//                 kp_servo_l -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str2, "kp_servo: %05d", kp_servo_l);
//             TFTSPI_dir_P6X8Str(2, 17, str2, u16RED, u16BLUE);
//             break;

//         case 4: // kd_servo_l
//             if (button2 == 0)
//             {
//                 kd_servo_l += 1;
//             }
//             else if (button3 == 0)
//             {
//                 kd_servo_l -= 1;
//             }
//             else if (button4 == 0)
//             {
//                 editingItem = false;
//             }
//             sprintf(str3, "kd_servo: %05d", kd_servo_l);
//             TFTSPI_dir_P6X8Str(2, 18, str3, u16RED, u16BLUE);
//             break;
//         }
//     }
// }

// void handleMOVE()
// {
//     int button4 = key4();


//     if (button4 == 0)
//     {
//         Servo_value(150000); // 舵机归中
//         LMotor_value(0);     // 电机停止
//         RMotor_value(0);
//         currentState = 0;
//         displayMAIN();
//     }
// }

// void displayMAIN()
// {
//     TFTSPI_dir_P6X8Str(0, 0, "MAIN MENU", u16WHITE, u16BLACK);
//     const char *menuItems[3] = {"IMG", "PID", "MOVE"};
//     for (int i = 0; i < 3; i++)
//     {
//         uint16_t color = (i == currentMenu) ? u16RED : u16WHITE;
//         TFTSPI_dir_P6X8Str(2, 12 + i, menuItems[i], color, u16BLACK);
//     }
// }

// void displayIMG()
// { /// 阈值 前瞻 目标速度
//     // 清屏
//     // TFTSPIClear(u16BLACK);
//     TFTSPI_dir_P6X8Str(2, 10, "IMG Menu", u16WHITE, u16BLACK);

//     Mat view = applyPerspectiveTransform(src);

//     Mat gray, binary;

//     cvtColor(view, gray, COLOR_BGR2GRAY);

//     Mat binary = binary_img(gray, i);

//     for (int i = 0; i < 120; i++)
//     {
//         for (int j = 0; j < 160; j++)
//         {
//             data[i][j] = binary.at<unsigned char>(i, j);
//         }
//     }

//     TFTSPI_dir_binRoad(0, 0, 120, 160, (uint8_t *)data);

//     const char *pidItems[3] = {"Threshold", "motor_goal_666", "qianzhan_dx"};
//     int pidValues[3] = {i, motor_goal_666, qianzhan_dx};

//     for (int i = 0; i < 3; i++)
//     {
//         uint16_t color = (i == currentIMG) ? u16RED : u16WHITE;
//         sprintf(str6, "%s: %05d", pidItems[i], pidValues[i]);
//         TFTSPI_dir_P6X8Str(2, 12 + i, str6, color, u16BLACK);
//     }
//     TFTSPI_dir_P6X8Str(2, 20, "B2/B3: Adjust Threshold", u16WHITE, u16BLACK);
//     TFTSPI_dir_P6X8Str(2, 21, "B4: Back", u16WHITE, u16BLACK);
// }

// void displayPIDMenu()
// {
//     TFTSPI_dir_P6X8Str(2, 10, "PID Menu", u16WHITE, u16BLACK);

//     // 显示PID参数
//     const char *pidItems[5] = {"kp_servo_f", "kpp", "kd_servo_f", "kp_servo_l", "kd_servo_l"};
//     int pidValues[5] = {kp_servo_f, kpp, kd_servo_f, kp_servo_l, kd_servo_l};

//     for (int i = 0; i < 5; i++)
//     {
//         uint16_t color = (i == currentPID) ? u16RED : u16WHITE;
//         sprintf(str6, "%s: %05d", pidItems[i], pidValues[i]);
//         TFTSPI_dir_P6X8Str(2, 12 + i, str6, color, u16BLACK);
//     }
//     // 显示操作提示
//     TFTSPI_dir_P6X8Str(2, 18, "B1: Select Item", u16WHITE, u16BLACK);
//     TFTSPI_dir_P6X8Str(2, 19, "B2/B3: Navigate", u16WHITE, u16BLACK);
//     TFTSPI_dir_P6X8Str(2, 20, "B4: Back", u16WHITE, u16BLACK);
// }

// void displayMOVEMenu()
// {
//     // 清屏
//     // TFTSPIClear(u16BLACK);

//     // 显示菜单标题
//     TFTSPI_dir_P6X8Str(2, 10, "MOVE Menu", u16WHITE, u16BLACK);

//     // 显示当前状态
//     sprintf(str2, "Servo Pos: %05d", servowyz);
//     TFTSPI_dir_P6X8Str(2, 12, str2, u16RED, u16BLUE);
//     sprintf(str3, "Motor Speed: %05d", L);
//     TFTSPI_dir_P6X8Str(2, 13, str3, u16RED, u16BLUE);
//     sprintf(str4, "Motor Speed: %05d", R);
//     TFTSPI_dir_P6X8Str(2, 14, str4, u16RED, u16BLUE);

//     // 显示操作提示
//     TFTSPI_dir_P6X8Str(2, 18, "Adjust PID in PID Menu", u16WHITE, u16BLACK);
//     TFTSPI_dir_P6X8Str(2, 20, "B4: Stop & Back", u16WHITE, u16BLACK);
// }